#include "troyglcontroller.h"

TroyGLController::TroyGLController(QObject* parent) :
    RobotGLController(parent),
    _recentCommand(NULL),
    _dragCommand(NULL),
    _prevX(0),
    _prevY(0),
    _prevZ(0),
    _mouseDrag(false),
    _changeOccured(false)
{
    for(int i = 0; i < MAX_NAMES; i++){
        _currentSelection[i] = 0;
    }
}

void TroyGLController::loadModel(Robot *robot){
    RobotGLController::loadModel(robot);
    connect(robot, SIGNAL(robotChanged()), this, SLOT(onRobotChanged()));
}


void TroyGLController::componentPicked(NameGroup names){
    //Have the view "select" the component (i.e., highlight it)
    if(_editingAllowed){
        int i;
        for(i = 0; i < MAX_NAMES; i++){
            _currentSelection[i] = names[i];
        }

        _view->selectComponent(names);
    }
}

void TroyGLController::noPick(){
    if(_editingAllowed){
        int i;
        for(i = 0; i < MAX_NAMES; i++){
            _currentSelection[i] = 0;
        }

        _view->clearSelection();
    }
}


void TroyGLController::mousePressEvent(QMouseEvent *event){
    //Begin tracking for dragging
    _mouseDrag = true;
    //Store the initial mouse position
    _view->transformTo3D((float)event->x(), (float)event->y(),
                         &_prevX, &_prevY, &_prevZ);

    _changeOccured = false;
}


void TroyGLController::mouseReleaseEvent(QMouseEvent* event){
    _mouseDrag = false;

    //Deselect the current selection
    if(_editingAllowed){
        _view->deselectComponent(_currentSelection);

        if(_changeOccured){
            qDebug() << "Change occured";
            emit editingFinished();
        }
    }
}

void TroyGLController::mouseMoveEvent(QMouseEvent *event){
    double posX, posY, posZ;
    _view->transformTo3D((float)event->x(), (float)event->y(),
                         &posX, &posY, &posZ);

    if(_mouseDrag){
        if(_editingAllowed){
            //Do some inverse kinematics to follow the cursor
            QVector3D goal(posX,posY,posZ);
            QList<int> jointIDs;
            if(_currentSelection[0] == Troy::T_LEFT_FOREARM){
                jointIDs.append(0); //Flexion
                jointIDs.append(1); //Abduction
                //jointIDs.append(8); //Shoulder
                jointIDs.append(9); //Elbow
                inverseKinematics(_currentSelection, goal, jointIDs);
            }
            else if(_currentSelection[0] == Troy::T_RIGHT_FOREARM){
                jointIDs.append(16); //Flexion
                jointIDs.append(17); //Abduction
                //jointIDs.append(24); //Shoulder
                jointIDs.append(25); //Elbow
                inverseKinematics(_currentSelection, goal, jointIDs);
            }
        }

        _prevX = posX;
        _prevY = posY;
        _prevZ = posZ;
    }
}

int TroyGLController::inverseKinematics(const NameGroup segmentNames, QVector3D goal,
                                         QList<int> jointIDs){

    int i, calculationCount = 0;
    const RobotComponent* component = _model->getComponent(segmentNames);
    const RobotSegment* segment;
    double prevDist, dist;
    QVector3D nullVector;
    double gradients[4];

    if(component != NULL  && component->type() == SEGMENT){
        segment = (RobotSegment*)component;

        //Start a batch so that the robot isn't emitting events
        //as we are testing the joint angles
        _model->startBatch();

        dist = segment->endPoint().distanceToLine(goal, nullVector);
        prevDist = 100;
        //Is absolute value needed?
        while(fabs(prevDist) > fabs(dist)){
            prevDist = dist;

            for(i = 0; i < jointIDs.length(); i++){
                gradients[i] = calcGradient(segment, goal, jointIDs[i]);
                calculationCount += 2;
            }

            for(i = 0; i < jointIDs.length(); i++){
                _model->moveJointBy(jointIDs[i], gradients[i]);
            }

            dist = segment->endPoint().distanceToLine(goal, nullVector);
            calculationCount++;
        }

        //TODO: Return one degree previously?
        _model->endBatch();

    }

    return calculationCount;
}


double TroyGLController::calcGradient(const RobotSegment* segment, QVector3D goal, int jointID){
    //This function calculates the gradient for changing a joint
    //as: gradient = (distance from end point to goal w/ joint angle + 1) -
    //               (distance from end point to goal w/ joint angle - 1)
    double gradient;
    double positiveDistance, negativeDistance;
    QVector3D endPoint, nullVector;

    _model->moveJointBy(jointID, 1);
    endPoint = segment->endPoint();
    positiveDistance = endPoint.distanceToLine(goal, nullVector);
    _model->moveJointBy(jointID, -2);
    endPoint = segment->endPoint();
    negativeDistance = endPoint.distanceToLine(goal, nullVector);
    _model->moveJointBy(jointID, 1);

    //Troy's joints can only move one degree at a time, so round to the nearest
    //integer value. This should drastically reduce the number of calculations needed
    //for the IK algorithm
    gradient = negativeDistance - positiveDistance;
    if(gradient > 0)
        gradient = ceil(gradient);
    else
        gradient = floor(gradient);

    return gradient;
}

void TroyGLController::onRobotChanged(){
    if(_mouseDrag){
        //We were dragging the robot and a change occurred
        _changeOccured = true;
    }
}
